#include "Lynx6.h"
#include "imageloader.h"

static int x1 = 0, x2 = 0, x3 = 0, x4= 0, x5 = 0;
float _angle = 0; 
int i, j;
int dof=5;
int renew= 0;
float _angleT2=0;
	
vector<float> RT(3);
vector<float> Angles( 5 + 3);

void PassParams(vector<float>& inAng) {	
	for( int i = 0; i <dof; i++ ) {
			Angles[i] = inAng[i];
	}
	RT[0] = inAng[5]; // translation x
	RT[1] = inAng[6]; // translation y
	RT[2] = inAng[7]; // rotation about z
}



// get the current param state (e.g. to check vs. tracked value)
void GetParams(vector<float>& outAng) {
	outAng = vector<float>(5+3);
	for( int i = 0; i < dof; i++)
		outAng[i] = Angles[i];
	outAng[5] = RT[0];
	outAng[6] = RT[1];
	outAng[7] = RT[2];
}

void light(void)
{

	//Ambient Light
	float ambientColor[]= {0.5,0.5,0.5, 0 };
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT, ambientColor);
	
	//Positioned Light 1
	float lightColor0[] = {.75f, .75f, .75f};
	float lightPos0[] = {5.0f, 5.0f, 5.0f, 1.0f};
	glLightfv(GL_LIGHT0, GL_DIFFUSE, lightColor0);
	glLightfv(GL_LIGHT0, GL_POSITION, lightPos0);

	//Directed Light 2
	float lightColor1[] = {.75f, .75f, .75f};
	float lightPos1[] = {-5.0f, 5.0f, 5.0f, 1.0f};
	glLightfv(GL_LIGHT1, GL_DIFFUSE, lightColor1);
	glLightfv(GL_LIGHT1, GL_POSITION, lightPos1);
	

}


void norm(vector<float>& x, vector<float>& y, vector<float> norm1)
{
vector<float> norm(3);
float val;

norm[0] = x[1]*y[2]-x[2]*y[1];
norm[1] = x[0]*y[2]-x[2]*y[0];
norm[2] = x[1]*y[0]-x[0]*y[1];
val = sqrt(double (norm[0]*norm[0]+norm[1]*norm[1]+norm[2]*norm[2]));
norm1[0] = norm[0]/val;
norm1[1] = norm[1]/val;
norm1[2] = norm[2]/val;
}

void drawCylinder(double h, double r1, double r2, int dR)
{
	int i,j;
	vector<float> run1(3);
	vector<float> run2(3);
	vector<float> run3(3);
	vector<float> run4(3);
	vector<float> run5(3);
	vector<float> run6(3);
	vector<float> x(3);
	vector<float> y(3);
	vector<float> x1(3);
	vector<float> y1(3);
	vector<float> x2(3);
	vector<float> y2(3);
	vector<float> nor(3);
	vector<float> nor1(3);
	vector<float> nor2(3);

	run5[0]=0;
	run5[1]=0;
	run5[2]=h;
	run6[0]=0;
	run6[1]=0;
	run6[2]=0;

	glPushMatrix();
	glBegin(GL_QUADS);
	for (i=0;i<dR;i++)
	{
		run1[0]=r1*cos(2*M_PI*i/dR);
		run1[1]=r1*sin(2*M_PI*i/dR);
		run1[2]=h;

		run2[0]=r2*cos(2*M_PI*i/dR);
		run2[1]=r2*sin(2*M_PI*i/dR);
		run2[2]=0;

		run3[0]=r1*cos(2*M_PI*(i+1)/dR);
		run3[1]=r1*sin(2*M_PI*(i+1)/dR);
		run3[2]=h;

		run4[0]=r2*cos(2*M_PI*(i+1)/dR);
		run4[1]=r2*sin(2*M_PI*(i+1)/dR);
		run4[2]=0;
		
		for (j=0;j<3;j++){
		x[j]=run1[j]-run2[j];
		y[j]=run4[j]-run2[j];
		}

		for (j=0;j<3;j++){
		x1[j]=run5[j]-run1[j];
		y1[j]=run3[j]-run1[j];
		}
		
		for (j=0;j<3;j++){
		x2[j]=run6[j]-run1[j];
		y2[j]=run4[j]-run2[j];
		}
		
		norm(x,y, nor);
		norm(x1,y1, nor1);
		norm(x2,y2, nor2);
		
	
	glNormal3f(nor[0],nor[1],nor[2]);
	glVertex3f(run1[0],run1[1],run1[2]);	
	glVertex3f(run3[0],run3[1],run3[2]);
	glVertex3f(run4[0],run4[1],run4[2]);
	glVertex3f(run2[0],run2[1],run2[2]);

	glNormal3f(-nor1[0],-nor1[1],-nor1[2]);
	glVertex3f(run1[0],run1[1],run1[2]);	
	glVertex3f(run5[0],run5[1],run5[2]);
	glVertex3f(run5[0],run5[1],run5[2]);
	glVertex3f(run3[0],run3[1],run3[2]);

	glNormal3f(-nor2[0],-nor2[1],-nor2[2]);
	glVertex3f(run2[0],run2[1],run2[2]);	
	glVertex3f(run6[0],run6[1],run6[2]);
	glVertex3f(run6[0],run6[1],run6[2]);
	glVertex3f(run4[0],run4[1],run4[2]);

	}
	glEnd();
	glPopMatrix();

}

// Draw a plane, if W is zero it goes in the XY plane
void drawPlain( float width, float height, vector<float>& W, vector<float>& C, vector<float>& pos)
{
	glPushMatrix();
	glTranslatef(pos[0],pos[1],pos[2]);
	glRotatef(W[0],1,0,0);
	glRotatef(W[1],0,1,0);
	glRotatef(W[2],0,0,1);	
	glScalef(width/10,height/10,1.0);	
	glBegin(GL_QUADS);
	glColor3f(C[0],C[1],C[2]);

	glNormal3f(0,0,1);
	glVertex3f(10,10,0);	
	glVertex3f(10,-10,0);
	glVertex3f(-10,-10,0);
	glVertex3f(-10,10,0);
	glEnd();
	glPopMatrix();

}


void drawPartLimb(float size)
{
	glPushMatrix();
	glScalef(size/14.7,size/14.7,size/14.7);
	glColor3f(.95,.75,0);

	glPushMatrix();
	glTranslatef(0,(14.431-2.465)/2,-.330/2);	
	drawCylinder(.330,2.465/2,2.465/2,20);
	glPopMatrix();

	glPushMatrix();
	glScalef(2.465,14.431-2.465,.330+.001);
	glutSolidCube(1);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(0,-(14.431-2.465)/2,-.330/2);	
	drawCylinder(.330,2.465/2,2.465/2,20);
	glPopMatrix();

	glPopMatrix();
}
void drawFrontLimb(float size)
{
	glPushMatrix();	
	glRotatef(180,1,0,0);
	glTranslatef(-1.685,-3.394,0);
	glScalef(size,size,size);
	glColor3f(.95,.85,0);
	glPushMatrix();
	glTranslatef(2.54*0.6,2.54*1.75,1/16*2.54);
	drawCylinder(2.54*1/8,2.54*.5,2.54*.5,10);
	glPopMatrix();
	glBegin(GL_QUADS);
	glNormal3f(0,0,1);
	glVertex3f(2.54*.5*sin(15*M_PI/180)+2.54*.6,2.54*1.75-2.54*.5*cos(15*M_PI/180),2.54*1/16);	
	glVertex3f(2.54*.5/2,2.54*.75/2,2.54*1/16);
	glVertex3f(-2.54*.5/2,2.54*.75/2,2.54*1/16);
	glVertex3f(2.54*.1,2.54*1.75,2.54*1/16);

	glNormal3f(0,0,-1);
	glVertex3f(2.54*.5*sin(15*M_PI/180)+2.54*.6,2.54*1.75-2.54*.5*cos(15*M_PI/180),-2.54*1/16);	
	glVertex3f(2.54*.5/2,2.54*.75/2,-2.54*1/16);
	glVertex3f(-2.54*.5/2,2.54*.75/2,-2.54*1/16);
	glVertex3f(2.54*.1,2.54*1.75,-2.54*1/16);

	glNormal3f(0,0,1);
	glVertex3f(2.54*.5*sin(15*M_PI/180)+2.54*.6,2.54*1.75-2.54*.5*cos(15*M_PI/180),-2.54*1/16);	
	glVertex3f(2.54*.5*sin(15*M_PI/180)+2.54*.6,2.54*1.75-2.54*.5*cos(15*M_PI/180),+2.54*1/16);
	glVertex3f(2.54*.5/2,2.54*.75/2,2.54*1/16);
	glVertex3f(2.54*.5/2,2.54*.75/2,-2.54*1/16);

	glNormal3f(0,0,1);
	glVertex3f(2.54*.1,2.54*1.75,2.54*1/16);	
	glVertex3f(2.54*.1,2.54*1.75,-2.54*1/16);
	glVertex3f(-2.54*.5/2,2.54*.75/2,-2.54*1/16);
	glVertex3f(-2.54*.5/2,2.54*.75/2,2.54*1/16);

	glEnd();
	glPushMatrix();
	glScalef(2.54*.5,2.54*.75,2.54*1/8);
	glutSolidCube(1);
	glPopMatrix();
	glPopMatrix();

}
void drawMidLimb(float size)
{
	glScalef(size,size,size);
	glPushMatrix();
	glScalef(2.75,.95,2);
	glTranslatef(0,-6,0);
	drawPartLimb(6.627);
	glPopMatrix();

	glPushMatrix();
	glColor3f(.95,.75,0);
	glScalef(1.988,6.192,.38);
	glutSolidCube(1);
	glPopMatrix();

	glPushMatrix();
	glScalef(2.75,.95,2);
	glTranslatef(0,6,0);
	drawPartLimb(6.627);
	glPopMatrix();
}
void drawMotor(float size, vector<float>& W,  vector<float>& pos)
{
	vector<float> dim(3);
	dim[0]=2.017;
	dim[1]=3.884;
	dim[2]=3.884;
	glPushMatrix();
	glScalef(size,size,size);
	glTranslatef(pos[0],pos[1],pos[2]);	
	glRotatef(W[0],1,0,0);
	glRotatef(W[1],0,1,0);
	glRotatef(W[2],0,0,1);	
		
	glPushMatrix();
	glTranslatef(0,dim[1]/2+.267,.25*dim[2]);
	glRotatef(-90,1,0,0);
	glColor3f(.95,.95,.95);
	drawCylinder(.267,2.391/2,2.391/2,20);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(0,dim[1]/2,.25*dim[2]);
	glRotatef(-90,1,0,0);
	glColor3f(.95,.95,.95);
	drawCylinder(.267,.889/2,.889/2,20);
	glPopMatrix();

	glPushMatrix();		
	glScalef(dim[0],dim[1],dim[2]);	
	glColor3f(.1,.1,.1);
	glutSolidCube(1);
	glPopMatrix();
	glPopMatrix();
}
void drawBase()
{

	glPushMatrix();
	glColor3f(.45,.35,.15);
	glTranslatef(3.3875+9.421/2,0,0);
	glPushMatrix();
	glScalef(26.035,1.868,13.856);
	glutSolidCube(1);
	glPopMatrix();

	glPushMatrix();
	glColor3f(.45,.35,.15);
	glTranslatef(13.0175,1.868,0);
	glScalef(5.734,3.621,13.851);
	glutSolidCube(1);
	glPopMatrix();

	glPushMatrix();
	glColor3f(.1,.1,.1);
	glTranslatef(1.4275,1.868,0);
	glScalef(11.393,.215,8.556);
	glutSolidCube(1);
	glPopMatrix();

	glPushMatrix();
	glColor3f(.0,.25,.0);
	glTranslatef(3.9155,3.529,0);
	glScalef(5.866,.218,7.624);
	glutSolidCube(1);
	glPopMatrix();
	glPopMatrix();

	glPushMatrix();
	glTranslatef(0,1.868,0);
	glRotatef(-90,1,0,0);
	glColor3f(.25,.25,.25);
	drawCylinder (6.041-1.863, 9.421/2, 9.421/2,20);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(0,6.041-1.863,0);
	glRotatef(-90,1,0,0);
	glColor3f(.25,.25,.25);
	drawCylinder (.389, 9.421/2, 9.421/2,20);
	glPopMatrix();

}

void drawBody()
{
	vector<float> pos1(3);
	vector<float> pos2(3);
	vector<float> pos3(3);
	vector<float> pos4(3);
	vector<float> pos5(3);
	vector<float> pos6(3);
	vector<float> W1(3);
	vector<float> W2(3);
	vector<float> W3(3);
	vector<float> W4(3);
	vector<float> W5(3);
	vector<float> W6(3);
	vector<float> size(6);
	pos1[0]=.25*9.421/2;
	pos1[1]=6.32+1.98/2;
	pos1[2]=9.421/2;
	pos2[0]=.25*9.421/2;
	pos2[1]=6.32+1.98/2;
	pos2[2]=-.267;
	pos3[0]=0;
	pos3[1]=11.5+6.65;
	pos3[2]=-.267;
	pos4[0]=0;
	pos4[1]=11.5+6.65+12.144;
	pos4[2]=-.267;
	pos5[0]=-1.591;
	pos5[1]=12.5+6.65+13.2+3.027+12;
	pos5[2]=-1*.267*3/4;
	pos6[0]=-3.884*.75*.25;
	pos6[1]=-2.017;
	pos6[2]=0;
	W1[0]=0;
	W1[1]=-90;
	W1[2]=90;
	W2[0]=0;
	W2[1]=-90;
	W2[2]=90;
	W3[0]=-90;
	W3[1]=0;
	W3[2]=0;
	W4[0]=-90;
	W4[1]=0;
	W4[2]=0;
	W5[0]=-180;
	W5[1]=0;
	W5[2]=-180;
	W6[0]=0;
	W6[1]=90;
	W6[2]=0;
	size[0]=1;
	size[1]=1;
	size[2]=1;
	size[3]=1;
	size[4]=.75;
	size[5]=.75;
	
	//base
	drawBase();

	//first DOF
	glPushMatrix();
	glRotatef(Angles[0],0,1,0);
	glPushMatrix();	
	drawMotor(size[0], W1, pos1 );
	drawMotor(size[1], W2, pos2 );
	glPopMatrix();


	//second DOF
	glPushMatrix();
	glTranslatef(0,7.4,0);
	glRotatef(Angles[1],0,0,1);
	glTranslatef(0,-7.4,0);
	glPushMatrix();
	glTranslatef(0,6.662+14.475/2-.65,-5.406/2);	
	drawPartLimb(14.475);
	glPopMatrix();
	glPushMatrix();
	glTranslatef(0,6.662+14.475/2-.65,5.406/2-2*.267);	
	drawPartLimb(14.475);
	glPopMatrix();



	//Third DOF
	glPushMatrix();
	glTranslatef(0,(6+13),0);
	glRotatef(Angles[2],0,0,1);
	glTranslatef(0,-(6+13),0);
	glPushMatrix();

	glPushMatrix();
	drawMotor(size[3], W4, pos4 );
	glPopMatrix();
	glPushMatrix();
	glTranslatef(0,11.5+6.65+6.394,2.447/2);
	drawMidLimb(1);
	glPopMatrix();
	glPushMatrix();
	glTranslatef(0,11.5+6.65+6.394,-2.447/2);
	drawMidLimb(1);
	glPopMatrix();
	glPushMatrix();
	drawMotor(size[2], W3, pos3 );
	glPopMatrix();
	glPopMatrix();


	//fourth DOF
	glPushMatrix();
	glTranslatef(0,(6+13+12),0);
	glRotatef(Angles[3],0,0,1);
	glTranslatef(0,-(6+13+12),0);
	glPushMatrix();
	glColor3f(.95,.85,0);
	glTranslatef(-1.591,12.5+6.65+13.2+3.027,-1*.267*3/4);
	glScalef(1/16*2.54,3.083,5.406);
	glutSolidCube(1);
	glPopMatrix();
	glPushMatrix();
	glTranslatef(0,12.5+6.65+13.2,5.452/2-.267);
	drawFrontLimb(1);
	glPopMatrix();
	glPushMatrix();
	glTranslatef(0,12.5+6.65+13.2,-5.452/2);
	drawFrontLimb(1);
	glPopMatrix();
	glPushMatrix();
	glScalef(.75,1,1);
	drawMotor(size[4], W5, pos5 );
	glPopMatrix();


	//fifth DOF
	glPushMatrix();
	glTranslatef(0,15*2.54,0);
	glRotatef(-90,1,0,0);	
	glRotatef(Angles[4]-90,0,0,1);
	glPushMatrix();
	
	
	glColor3f(.1,.1,.1);
	glTranslatef(0,0,8.6/2);
	glScalef(2.5,.329,8.6);
	glutSolidCube(1);
	glPopMatrix();
	glPushMatrix();
	glColor3f(.1,.1,.1);
	glTranslatef(0,0,2.412);
	glRotatef(-10,0,1,0);
	glTranslatef(.956,.486,2.46/4);
	glScalef(.64,.329,1.979);
	glutSolidCube(1);
	glPopMatrix();
	glPushMatrix();
	glColor3f(.1,.1,.1);
	glTranslatef(0,0,2.412);
	glRotatef(10,0,1,0);
	glTranslatef(-1-.956,.486,2.46/4);
	glScalef(.64,.329,1.979);
	glutSolidCube(1);
	glPopMatrix();
	glPushMatrix();
	glColor3f(.1,.1,.1);
	glTranslatef(0,.267*2,1.961);
	glRotatef(-90,1,0,0);
	drawCylinder(.347,1.8/2,1.8/2,20);
	glPopMatrix();
	glPushMatrix();
	glColor3f(.1,.1,.1);
	glTranslatef(-.25*5.448,.267*2,1.961);
	glRotatef(-90,1,0,0);
	drawCylinder(.347,1.8/2,1.8/2,20);
	glPopMatrix();
	glPushMatrix();
	glColor3f(.1,.1,.1);
	glScalef(3.369,3.185,.710);
	glutSolidCube(1);
	glPopMatrix();
	glPushMatrix();
	glTranslatef(-.25*5.448,0,.710/2+2.2232/2);
	glColor3f(.95,.85,.0);
	glScalef(5.448,.359,2.2232);
	glutSolidCube(1);
	glPopMatrix();
	glPushMatrix();
	glTranslatef(-.25*5.448,0,.710/2+2.2232/2);
	drawMotor(size[5], W6, pos6);
	glPopMatrix();
	glPopMatrix();
	glPopMatrix();
	glPopMatrix();
	glPopMatrix();
	glPopMatrix();
	glPopMatrix();
















	
}


void drawArm()
{

	vector<float> size(3);
	vector<float> pos1(3);
	vector<float> pos2(3);
	vector<float> C1(3);
	vector<float> C2(3);
	vector<float> W1(3);
	vector<float> W2(3);
	
	
	//define axis dimensions
	//define them to scale with the size of
	//our calibration object!
	// the value they multiply is 1.8
	// for the AXES object we have coordinate "12" for x,y,z
	size[0]=12 / 1.8;
	size[1]=12 / 1.8;
	size[2]=12 / 1.8;




	//define pos of the background
	//pos1[2]=-50;
	//define Color of the background
	C1[2]=.75;
	//define Color of the ground
	C2[0]=.75;
	C2[1]=.75;
	C2[2]=.75;
	//define Orientation of the ground
	//W1[0]=-90;

	

	//necessary to begin the production of the structures
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	light();
	
	// update PK: the axes need to match what
	// what was in the calibration object.

	//floor axis
	
	glPushMatrix();
	
	glRotatef(-90,0,1,0);
	glRotatef(-90,1,0,0);
	DrawAxes(size);	
	vector<float> calibPts(13*3);
	for(int i = 0; i < 3*4; i+=3 ) {
		calibPts[i] = 0;
		calibPts[i+1] = 3 + i;
		calibPts[i+2] = 0;
	}
	for( int i = 0; i < 3*5; i+=3 ) {
		calibPts[12+i] = 3 + i;
		calibPts[12+i+1] = 0;
		calibPts[12+i+2] = 0;
	}
	for( int i = 0; i < 3*4; i+=3 ) {
		calibPts[27+i] = 0;
		calibPts[27+i+1] = 0;
		calibPts[27+i+2] = 3 + i;
	}
	DrawCalibrationPoints(calibPts);
	pos2[2] = -5;
	
	glPushMatrix();
		// Note: Something is weird, like the y-coordinates are not being
	    // flipped in openGL when they were in matlab
	glRotatef(90,1,0,0);
	glRotatef(-45,0,1,0);
	glTranslatef(0,0,5);
	drawPlain(100,100,W1,C1,pos1); // blue background
	glPopMatrix();

	drawPlain(100,100,W2,C2,pos2); // ground
	//glPopMatrix();

	glScalef(0.5,0.5,0.5);
	glPushMatrix(); // the robot arm relative to calibration object world coordinates
		glTranslatef(11,-3,-1);
		//glRotatef(Angles[0],0,1,0);
		glTranslatef(RT[0],RT[1],0.0);
		glRotatef(RT[2],0,0,1);
		
		glRotatef(90,1,0,0);
		glRotatef(90,0,1,0);
		drawBody();
	glPopMatrix();
	glPopMatrix();
	
}
